/*
 * Astrolabe.java
 *
 * Created on March 8, 2007, 4:33 PM
 */

package astrolabe;

import astrolabe.imgprocessing.*;
import java.awt.*;
import java.awt.geom.Line2D;
import java.awt.image.BufferedImage;
import java.util.LinkedList;
import javax.media.jai.PlanarImage;
import org.jfarrell.classes.Command;

/**
 *
 * @author Tobias Knutsson
 */
public class Astrolabe {
    
    VisorViewer vv;
    
    Command cc=null;
    
    private static final float BOT_LEN   = 16.0F;
    private static final float BOT_SCALE =  0.5F;
    private static final float MAP_SCALE =  0.2F;
    private static final int   BOT_PAD   =     6;

    private static final float   POS_SCALE   =     0.89f;
    private static final float   NEG_SCALE   =     0.85f;

    
    
    public Astrolabe() {
        /* [.] Not started 
         * [-] Some progress
         * [+] Done
         * ---------------------------------------
         * + Receive data via socket
         * + Assemble data to objects
         * + Convert AwtImage to PlanarImage
         * + Get the map (getMap)
         * + Get the position of the bot (findBot)
         * + Get initial rotation
         * + Remove the bot from the map
         * + Calculate path
         * + Connect and send commands to the RCX
         *----------------------------------------
         */
        vv = new VisorViewer();

        CmdServer cs = new CmdServer(2222, this);
            cs.start();
        
        org.pricejd.server.RCXProtocol rcx = new org.pricejd.server.RCXProtocol();
        PlanarImage img;
        BufferedImage processed;
        Line2D botLine;
        LinkedList<Point> path = new LinkedList();
        
        boolean[][] map;
        Point       botPos = new Point(0,0);
        Point       endPos;
        double      initialRot;
        double      lu2cm =1;
     
        Command c;

        while(true)
        {
            while(cc==null);
            
            c=cc;
            cc=null;
            
            vv.Log("Astrolabe: Got Command("+String.valueOf(c.getCmd())+")");
            
            switch(c.getCmd())
            {
                case 'p':   // Calculate a path and go there
                {
                    vv.Log("Visor: Starting image processing..");
                    img         = Visor.AwtToPlanar(c.getImage());
                    botLine     = Visor.findBot(img, BOT_SCALE, MAP_SCALE);
                    botPos      = Visor.getBotPos(botLine);
                    initialRot  = Visor.getInitialRot(botLine);                   
                    processed   = Visor.process(img, MAP_SCALE, BOT_PAD, botLine, initialRot);
                    map         = Visor.makeMap(processed);
                    
                    lu2cm       = BOT_LEN/(Math.hypot((double)(botLine.getX2()-botLine.getX1()),(double)(botLine.getY2()-botLine.getY1())));
                    endPos      = new Point((int)(c.getPoint().getX()*MAP_SCALE), (int)(c.getPoint().getY()*MAP_SCALE));
                    vv.Log("Visor: Image processing done");
                    vv.Log("Pathfinder: Starting pathfinding..");
                    pathfinding.PathFinder pf = new pathfinding.PathFinder(map, botPos, endPos);
                    
                    try
                    {
                        path = pf.calculatePath();
                        Point first = path.removeFirst();
                        int angle = first.y+(int)initialRot;
                        
                        if(angle>180)
                            angle-=360;
                        if(angle<-180)
                            angle+=360;
                        
                        path.addFirst(new Point(0,angle));
                    }catch(NullPointerException e)
                    {
                        vv.Log("Pathfinder: ERROR (Could not find path)");
                    }                    
                    vv.Log("Pathfinder: Path calculated");
                    
                    vv.setBotLine(new Line2D.Double((int)botLine.getX1()/(MAP_SCALE), (int)botLine.getY1()/(MAP_SCALE), (int)botLine.getX2()/(MAP_SCALE), (int)botLine.getY2()/(MAP_SCALE)));
                    vv.setProcessed(processed);
                    vv.setPath(pf.thePath);
                    vv.setImage(img.getAsBufferedImage());
                    vv.rePaint();
                    
                    for(Point p: path)
                    {
                        if(p.getX()==0 && p.getY()!=0)
                        {
                            vv.Log("RCX: Rotate("+p.getY()+")");
                            
                            if(p.y<0)
                                rcx.rotate((int)(p.getY()*NEG_SCALE));
                            else
                                rcx.rotate((int)(p.getY()*POS_SCALE));
                            
                            vv.Log("RCX: Rotation confirmed");
                        }
                        if(p.getX()==1)
                        {
                            vv.Log("RCX: Travel("+((p.getY()/10)*lu2cm)+")");
                            rcx.travel((int)(p.getY()*lu2cm/10));
                            vv.traveled();
                            vv.rePaint();
                            vv.Log("RCX: Travel confirmed");
                        }
                        
                        try {
                            
                            Thread.sleep(400);
                        } catch (InterruptedException ex) {
                            ex.printStackTrace();
                        }
                    }
                }; break;

                case 'f':   // move forward
                {
                    vv.Log("RCX: Forward(10)");
                    rcx.travel(10);
                }; break;

                case 'b':   // move backward
                {
                    vv.Log("RCX: Backward(10)");
                    rcx.travel(-10);
                }; break;

                case 'g':   // move backward
                {
                    vv.Log("RCX: Travel("+c.getVal()+")");
                    rcx.travel(c.getVal());
                }; break;

                case 't':   // move backward
                {
                    vv.Log("RCX: Rotate("+c.getVal()+")");
                    if(c.getVal()<0)
                        rcx.rotate((int)(c.getVal()*NEG_SCALE));
                    else
                        rcx.rotate((int)(c.getVal()*POS_SCALE));
                }; break;
                                                
                default:
                {
                    vv.Log("Astrolabe: ERROR(Unknown command)");
                }
            }
            vv.Log("Astrolabe: Done with current command");

        }
    }
    
    public static void main(String args[]) {
        Astrolabe aLabe = new Astrolabe();
    }
    
}
